/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "foc_model.h"
#include "foc_park_clark.h"
#include "foc_pid.h"
#include "drv_encoder.h"
#include "drv_time.h"
#include "foc_filter.h"
#include "foc_para.h"
#include "FreeRTOS.h"
#include "foc_tune.h"
#include "foc_util.h"
#include <string.h>
#include <math.h>

#define FOC_RESIZE_CUR(x) (FOC_ABS(x, 0) * 4096 / (400))

FOC_PID_S PidQ;
FOC_PID_S PidD;

DIM_ROTATE_S TargetQDCur = {20, 0};
DIM_TWO_S OpenAlphaBetaVoltage = {100, 0};//for encoder locate dect

void focModel(int32_t ia, int32_t ib, int32_t ic)
{
    DIM_THREE_S cur_three = {ia, ib, ic};
    DIM_TWO_S cur_two;
    DIM_ROTATE_S cur_rotate;
    static DIM_ROTATE_S cur_rotate_filter = {0, 0};
    DIM_ROTATE_S vol_rotate;
    DIM_TWO_S vol_two;
    DIM_THREE_S duty;
    int32_t phase = focConvertPhase(getCurPahse());
    phase = (phase * MotorInherentPara.PolePairs) % ANGLE2PAHSE(360);
    switch(foc_getSector(phase)) {
        case 4:
        case 5:
            cur_three.c = -cur_three.a - cur_three.b;
            break;
        case 6:
        case 1:
            cur_three.a = -cur_three.b - cur_three.c;
            break;
        case 2:
        case 3:
            cur_three.b = -cur_three.a - cur_three.c;
            break;
    }
    //trans origin current (uvw) to iq & id
    clarkTransform(cur_three, &cur_two);
    parkTransform(cur_two, phase, &cur_rotate);
    //filter and pid compute    
    cur_rotate_filter.d = lowPassFilter1Order(800, cur_rotate.d, cur_rotate_filter.d);
    cur_rotate_filter.q = lowPassFilter1Order(800, cur_rotate.q, cur_rotate_filter.q);
    vol_rotate.d = pidCtrlBlock(TargetQDCur.d, cur_rotate_filter.d, &PidD);
    vol_rotate.q = pidCtrlBlock(TargetQDCur.q, cur_rotate_filter.q, &PidQ);
    focTuneSetRefer(FOC_RESIZE_CUR(TargetQDCur.q));
    focTuneRealtimeOutput(FOC_RESIZE_CUR(vol_rotate.q));
    //trans vq & vd to real voltage (uvw)
    reserveParkTransform(vol_rotate, phase, &vol_two);
    svpwmTransform(vol_two, &duty);
    //apply duty to advtimer
    updataPwmDuty(duty.a, duty.b, duty.c);
}

void focInit(void)
{
    memset(&PidQ, 0, sizeof(FOC_PID_S));
    memset(&PidD, 0, sizeof(FOC_PID_S));
    PidQ.Kp = 200;
    PidQ.Ki = 400;
    PidQ.Kd = 0;
    PidQ.IntegralMax = 500;
    PidQ.IntegralMin = -500;
    PidQ.OutputMax = 500;
    PidQ.OutputMin = -500;
    PidQ.Divide = 1024;

    PidD.Kp = 200;
    PidD.Ki = 400;
    PidD.Kd = 0;
    PidD.IntegralMax = 500;
    PidD.IntegralMin = -500;
    PidD.OutputMax = 500;
    PidD.OutputMin = -500;
    PidD.Divide = 1024;
}

void focDeinit(void)
{
    
}

void focUpdateCurrQ(int16_t QValue)
{
    TargetQDCur.q = QValue;
}

int16_t focConvertPhase(int16_t originpahse)
{
    int16_t phase;    
    if(MotorWorkPara.LocateEnabled == false) {
        phase = (originpahse - MotorWorkPara.EncoderOriginPos) * \
            MotorWorkPara.EncoderDir;
        if(phase < 0)
            phase += 6280;//一圈为2π
        else if(phase > 6280)
            phase -+ 6280;
    } else {
        phase = MotorWorkPara.TargetPhase;
    }
    return phase;
}

void focLocateCtrl(void)
{
    int16_t tempphase[2] = {0};
    int16_t angle[2] = {0, 90};
    DIM_TWO_S voltemp[2] = {{100, 0}, {0, 100}};
    int16_t anglediff = 0;
    int8_t dir = 0;
    DIM_THREE_S duty;
    stopPwm();
    //turn motor 90°
    for(uint8_t i = 0;i < 2; i++) {
        motorSetPhase(ANGLE2PAHSE(angle[i]));
        OpenAlphaBetaVoltage.alpha = voltemp[i].alpha;
        OpenAlphaBetaVoltage.beta = voltemp[i].beta;
        svpwmTransform(OpenAlphaBetaVoltage, &duty);
        updataPwmDuty(duty.a, duty.b, duty.c);
        startPwm();
        msleep(1000);
        stopPwm();
        msleep(100);
        tempphase[i] = getCurPahse();
    }
    //compute dir and origin pos
    if(tempphase[0] > tempphase[1]) {
        anglediff = tempphase[0] - tempphase[1];
        dir = -1;
    } else {
        anglediff = tempphase[1] - tempphase[0];
        dir = 1;
    }

    if(anglediff > ANGLE2PAHSE(180)) {//origin is around 0°
        dir = dir * (-1);
    }
    MotorWorkPara.AccumulatePhase = 0;
    motorSetEncoterLocate(dir, tempphase[0]);    
}

